from timeloop import Timeloop
from datetime import timedelta
from linuxfd import timerfd
import time
from driver import lcm_subscriber,lcm_publisher
import lcm
import pybullet as p
import time
import pybullet_data
import torch
import math

# i=0
# j=0
#20220413 uniformly generate random quaternions using torch
# @torch.jit.script
# def torch_rand_float(lower, upper, shape, device):
#     # type: (float, float, Tuple[int, int], str) -> Tensor
#     return (upper - lower) * torch.rand(*shape, device=device) + lower
#
# i=torch.ones((10,1),device='cuda')
# u=torch_rand_float(0,1,(10,1),device='cuda')
# v=torch_rand_float(0,1,(10,1),device='cuda')
# w=torch_rand_float(0,1,(10,1),device='cuda')
# #h = ( sqrt(1-u) sin(2πv), sqrt(1-u) cos(2πv), sqrt(u) sin(2πw), sqrt(u) cos(2πw))
# x=torch.sqrt(i-u)*torch.sin(2*math.pi*v)
# y=torch.sqrt(i-u)*torch.cos(2*math.pi*v)
# z=torch.sqrt(u)*torch.sin(2*math.pi*w)
# o=torch.sqrt(u)*torch.cos(2*math.pi*w)
#
# h=torch.cat((x,y,z,o),dim=-1)
# m=torch_rand_float(-0.5, 0.5, (10, 6), device='cuda')

def _logistic_kernel(x):
    return -torch.reciprocal(torch.add(torch.exp(x), torch.exp(-x)) + 2)
h=torch.randn((10,15))
print(h)
o=torch.cat((h[:,3:6],h[:,9:]),dim=-1)
print(o)
# idx=(torch.norm(h,dim=-1)>1.3)
# print(idx)
# a=torch.norm(h[:,(0,2)]-h[:,(1,3)],dim=-1)
# a=h[:,3:]
# b=h[:,:3]
# print(a)
# print(b)
# c=torch.zeros(5,4,3)
# print(c)
# d=torch.zeros(5,4,*[3])
# print(d)
# print(_logistic_kernel(a))
# print(torch.sum(a,dim=-1))
# print(torch.sum(a,dim=-1)*idx)


# # sub=lcm_subscriber("test_subscriber",0.001,"MotorsTorquePB")
# pub=lcm_publisher("test_publisher",0.001,"MotorsInfoPB")
# # sub.start()
# pub.start()
# # sub.start()
#
# time.sleep(100)
# # sub.stop()
# pub.stop()
# sub.stop()

# Test timeloop
# tl = Timeloop()
# @tl.job(interval=timedelta(milliseconds=1))
# def print_everty_1ms():
#     global i,j
#     if i>1000:
#         i=0
#         j=j+1
#     print(j,":",i)
#     i=i+1

# tl.start(block=True)

# Test timerfd
# timer_fd=timerfd()
# timer_fd.settime(1,1)
#
# while 1:
#
#     time.sleep(0.5)
#
#     print(i)
#     i+=1
#     timer_fd.read()




